package com.lucenten.collect.gnss.handler.files;

import cn.quevo.common.lang.DateFormatUtils;
import cn.quevo.common.lang.NumberUtils;
import com.lucenten.collect.gnss.entity.TecpTecl;
import com.lucenten.collect.gnss.entity.trans.TecpTeclTrans;
import com.lucenten.collect.gnss.handler.tools.GnssContext;
import com.lucenten.collect.gnss.handler.tools.compute.FactorialCalculator;
import com.lucenten.collect.gnss.handler.tools.compute.Martrix;
import com.lucenten.collect.gnss.handler.tools.compute.RungeKutta;
import lombok.extern.slf4j.Slf4j;

import java.text.DecimalFormat;
import java.text.ParseException;
@Slf4j
public class GAzimuthElevation extends AzimuthElevation {
    // 系数
    private final static double WGS_COE = 1 + 22 * Math.pow(10, -9);
    // 幂
    private final static double NEG_POW6 = Math.pow(10, -6) * WGS_COE;
    private final static double[][] WGS_XYZk = {
            {
                    1.0 * WGS_COE,
                    -1.728 * NEG_POW6,
                    -0.017 * NEG_POW6
            },
            {
                    1.728 * NEG_POW6,
                    1 * WGS_COE,
                    0.076 * NEG_POW6
            },
            {
                    0.017 * NEG_POW6,
                    -0.076 * NEG_POW6,
                    1 * WGS_COE
            }
    };
    private final static double[] TEM = {-0.47, -0.51, -1.5};
    private final static double POW3 = Math.pow(10, 3);
    //偏心率%%%CGCS 2000：0.0818191910428158  GRS 80： 0.0818191910428318  WGS 84：0.0818191908426215
    private final static double e = Math.sqrt(0.00669437999013);

    /**
     * 计算仰角 方位角
     */
    public static void compute(TecpTeclTrans tecpTeclTrans, Double[] singlePath, TecpTecl tecpTecl) {
        if (singlePath == null) {
            return;
        }
        try {
            double xySqrt = Math.sqrt(tecpTecl.getY() * tecpTecl.getY() + tecpTecl.getX() * tecpTecl.getX());
            //（1）计算观测站在WGS-84大地坐标系下的坐标
            //L、B和H分别是观测站在大地坐标系中的经度、纬度和高程
            //（2）卫星在WGS-84坐标系下的空间直角坐标XK，YK和ZK,表示为站心地平直角坐标系中直角坐标x、y和z
            //double[][] TransC= {{N + H,N + H,N},{Math.cos(B),Math.cos(B),1.0 - e2 * e2},{Math.cos(L),Math.sin(L),Math.sin(B)}};

            double B = Math.atan(tecpTecl.getZ() / xySqrt); //给定一个初始值
            double L = Math.atan2(tecpTecl.getY(), tecpTecl.getX());
            double B0 = 0;
            double N = 0;
            double H = 0;
            while (Math.abs(B - B0) > 1e-10) {
                B0 = B;
                N = GnssContext.EARTH_RADIUS / (Math.sqrt(1 - e * e * Math.sin(B) * Math.sin(B)));            // %%%曲率半径
                H = (tecpTecl.getZ() / Math.sin(B)) - N * (1 - e * e);                          // %%%高程
                B = Math.atan(tecpTecl.getZ() * (N + H) / (xySqrt * (N * (1 - e * e) + H)));    // %%%%纬度
            }
            double B1 = B * 180 / Math.PI;               // %%%%纬度
            double L1 = Math.atan2(tecpTecl.getY(), tecpTecl.getX()) * 180 / Math.PI;// %%%%经度
            double H1 = H;                           // %%%%高程
            // %%计算仰角和方位角所需参量1
            double[][] transA = {{-Math.sin(B) * Math.cos(L), -Math.sin(L), Math.cos(B) * Math.cos(L)}, {-Math.sin(B) * Math.sin(L), Math.cos(L), Math.cos(B) * Math.sin(L)}, {Math.cos(B), 0, Math.sin(B)}};
            double[] transC = {(N + H) * Math.cos(B) * Math.cos(L), (N + H) * Math.cos(B) * Math.sin(L), (N * (1 - e * e) + H) * Math.sin(B)};
            //double[][] WGS_XYZk={{Eph[satNum][3],Eph[satNum][4],Eph[satNum][5]},{Eph[satNum][6],Eph[satNum][7],Eph[satNum][8]},{Eph[satNum][9],Eph[satNum][10],Eph[satNum][11]}};
            //（3）计算观测站相对于卫星的距离r、仰角E0和方位角AZ
            //方位角
            //double azi = Get_atan(x, y) * 180.0 / Math.PI;
            double[] pathData = convertPz90ToWgs82(singlePath, tecpTeclTrans.getDateTime());

            //矩阵计算类
            FactorialCalculator calculator = new FactorialCalculator();
            //计算矩阵求逆
            Martrix martrix = new Martrix();
            double[][] vals = martrix.getInvMatrix(transA);
            double[] wgsTrans = calculator.multiply(vals, calculator.minus(pathData, transC));

            double azi = Math.atan2(wgsTrans[1], wgsTrans[0]) * 180.0 / Math.PI;
            if (azi < 0) {
                azi = azi + 360;
            }
            //仰角
            double ele = Math.atan(wgsTrans[2] / Math.sqrt(wgsTrans[0] * wgsTrans[0] + wgsTrans[1] * wgsTrans[1])) * 180.0 / Math.PI;
            tecpTecl.setAzimuth(NumberUtils.toDouble(new DecimalFormat("0.0000").format(azi)));
            tecpTecl.setElevation(NumberUtils.toDouble(new DecimalFormat("0.0000").format(ele)));
        } catch (NumberFormatException a) {
        } catch (Exception e) {
            log.error("仰角计算失败",e);
        }
    }

    /**
     * GLONASS卫星 PZ90转WGS82坐标系
     *
     * @return
     * @throws ParseException
     * @author July july_sky@foxmail.com
     * @date 2019/5/3 18:08
     */
    private static double[] convertPz90ToWgs82(Double[] singlePath, final String dateTime) {
        //计算当前卫星的时间与当前时刻数据相差多少秒 并进行迭代
        // G 文件时间
        long beginTime = singlePath[16].longValue() / 1000l;
        // O文件时间
        long endTime = DateFormatUtils.parse(dateTime, "yyyyMMddHHmmss").getTime() / 1000;

        // Ax.Ay,Az=加速度; Vx,Vy,Vz = 速度
        //获取Vx/Ax、Vy/Ay、Vz/Az,存入tmp中
        double[] tmp = {
                singlePath[3] * POW3,
                singlePath[7] * POW3,
                singlePath[11] * POW3,
                singlePath[4] * POW3,
                singlePath[8] * POW3,
                singlePath[12] * POW3
        };
        double[] trans = new double[3];
        trans[0] = tmp[0];
        trans[1] = tmp[1];
        trans[2] = tmp[2];
        if (beginTime == endTime) {
            return trans;
        }


        /*if (Math.abs(endTime - beginTime) <= GnssContext.INTERVAL) {
            return trans;
        }*/
        int interval = GnssContext.INTERVAL;
        if (beginTime > endTime) {
            interval = - GnssContext.INTERVAL;
        }
        //获取当前卫星的xyz
        double xls = singlePath[5] * POW3;
        double yls = singlePath[9] * POW3;
        double zls = singlePath[13] * POW3;
        // 时间间隔(暂时全部采用30秒)
//        int interval = beginTime <= endTime ? GnssContext.INTERVAL : GnssContext.INTERVAL * -1;
        //根据龙格库塔算法进行迭代xyz
        RungeKutta kutta = new RungeKutta();
        kutta.rungeKutta(beginTime, endTime, interval, tmp, xls, yls, zls);
        FactorialCalculator calculator = new FactorialCalculator();
        if (kutta.getY1().length <= 1) {
            return trans;
        }
        double[][] vals = calculator.multiply(kutta.getY1(), interval, 1, kutta.getY1().length, 3, 6);
        double[] sPath = calculator.sum(vals);
        sPath = calculator.plus(trans, sPath);
        sPath = calculator.multiply(WGS_XYZk, sPath);
        sPath = calculator.plus(sPath, TEM);
        return sPath;
    }
}
